#include "Cmatrix.h"

float GetMatVal(Matrix* mat, size_t row, size_t col)
{
    return mat->data[row * mat->col + col];
}

void SetMatVal(Matrix* mat, size_t row, size_t col, float val)
{
    mat->data[row * mat->col + col] = val;
}

void MatrixMultiply(Matrix* left, Matrix* right, Matrix* res)
{
    for (size_t i = 0; i < left->row; i++)
    {
        for (size_t j = 0; j < right->col; j++)
        {
            float tmp = 0.0f;
            for (size_t z = 0; z < left->col; z++)
            {
                tmp += GetMatVal(left, i, z) * GetMatVal(right, z, j);
            }
            SetMatVal(res, i, j, tmp);
        }
    }
}

void MatrixPlus(Matrix* left, Matrix* right, Matrix* res)
{
    for (size_t i = 0; i < left->row; i++)
    {
        for (size_t j = 0; j < left->col; j++)
        {
            float tmp = GetMatVal(left, i, j) + GetMatVal(right, i, j);
            SetMatVal(res, i, j, tmp);
        }
    }
}

void MatrixMinus(Matrix* left, Matrix* right, Matrix* res)
{
    for (size_t i = 0; i < left->row; i++)
    {
        for (size_t j = 0; j < left->col; j++)
        {
            float tmp = GetMatVal(left, i, j) - GetMatVal(right, i, j);
            SetMatVal(res, i, j, tmp);
        }
    }
}

void MatrixSelfTranform(Matrix* Mat)
{
    if (Mat->col != Mat->row)return;
    for (size_t i = 0; i < Mat->row; i++)
    {
        for (size_t j = i + 1; j < Mat->col; j++)
        {
            float tmp = GetMatVal(Mat, i, j);
            SetMatVal(Mat, i, j, GetMatVal(Mat, j, i));
            SetMatVal(Mat, j, i, tmp);
        }
    }
}

void MatrixTranform(Matrix* Mat, Matrix* res)
{
    if (Mat->col != Mat->row)return;
    for (size_t i = 0; i < Mat->row; i++)
    {
        for (size_t j = 0; j < Mat->col; j++)
        {
            SetMatVal(res, i, j, GetMatVal(Mat, j, i));
        }
    }
}

void MatrixMultiplyNum(Matrix* Mat, float Num, Matrix* res)
{
    for (size_t i = 0; i < Mat->row; i++)
    {
        for (size_t j = 0; j < Mat->col; j++)
        {
            SetMatVal(res, i, j, GetMatVal(Mat, i, j) * Num);
        }
    }
}

void MatrixDivisionNum(Matrix* Mat, float Num, Matrix* res)
{
    for (size_t i = 0; i < Mat->row; i++)
    {
        for (size_t j = 0; j < Mat->col; j++)
        {
            SetMatVal(res, i, j, GetMatVal(Mat, i, j) / Num);
        }
    }
}

void MatrixPlusNum(Matrix* Mat, float Num, Matrix* res)
{
    for (size_t i = 0; i < Mat->row; i++)
    {
        for (size_t j = 0; j < Mat->col; j++)
        {
            SetMatVal(res, i, j, GetMatVal(Mat, i, j) + Num);
        }
    }
}

void MatrixMinusNum(Matrix* Mat, float Num, Matrix* res)
{
    for (size_t i = 0; i < Mat->row; i++)
    {
        for (size_t j = 0; j < Mat->col; j++)
        {
            SetMatVal(res, i, j, GetMatVal(Mat, i, j) - Num);
        }
    }
}


void ShowMat(Matrix* Mat)
{
    for (size_t i = 0; i < Mat->row; i++)
    {
        for (size_t j = 0; j < Mat->col; j++)
        {
            printf("%.2f\t", GetMatVal(Mat, i, j));
        }
        printf("\n");
    }
}



float LinealgVecModulus(Matrix* Vector)
{
    float res = 0.0f;
    for (size_t i = 0; i < Vector->row; i++)
    {
        for (size_t j = 0; j < Vector->col; j++)
        {
            res += GetMatVal(Vector, i, j) * GetMatVal(Vector, i, j);
        }
    }
    return sqrt(res);
}

void LinealgNorm(Matrix* VectorSrc, Matrix* VectorRes)
{
    float lenth = LinealgVecModulus(VectorSrc);
    MatrixDivisionNum(VectorSrc, lenth, VectorRes);
}

float LinealgPointMult(Matrix* VectorLeft, Matrix* VectorRight)
{
    float res = 0.0f;
    for (size_t i = 0; i < VectorLeft->row; i++)
    {
        for (size_t j = 0; j < VectorLeft->col; j++)
        {
            res += GetMatVal(VectorLeft, i, j) * GetMatVal(VectorRight, i, j);
        }
    }
    return res;
}

void LinealgCrossMult(Matrix* VectorLeft, Matrix* VectorRight, Matrix* VectorRes)
{
    if (VectorLeft->row == 1)
    {
        float tmp = 0.0f;
        tmp = GetMatVal(VectorLeft, 0, 1) * GetMatVal(VectorRight, 0, 2)
            - GetMatVal(VectorLeft, 0, 2) * GetMatVal(VectorRight, 0, 1);
        SetMatVal(VectorRes, 0, 0, tmp);

        tmp = -GetMatVal(VectorLeft, 0, 0) * GetMatVal(VectorRight, 0, 2)
            + GetMatVal(VectorLeft, 0, 2) * GetMatVal(VectorRight, 0, 0);
        SetMatVal(VectorRes, 0, 1, tmp);

        tmp = GetMatVal(VectorLeft, 0, 0) * GetMatVal(VectorRight, 0, 1)
            - GetMatVal(VectorLeft, 0, 1) * GetMatVal(VectorRight, 0, 0);
        SetMatVal(VectorRes, 0, 2, tmp);
    }
    else
    {
        float tmp = 0.0f;
        tmp = GetMatVal(VectorLeft, 1, 0) * GetMatVal(VectorRight, 2, 0)
            - GetMatVal(VectorLeft, 2, 0) * GetMatVal(VectorRight, 1, 0);
        SetMatVal(VectorRes, 0, 0, tmp);

        tmp = -GetMatVal(VectorLeft, 0, 0) * GetMatVal(VectorRight, 2, 0)
            + GetMatVal(VectorLeft, 2, 0) * GetMatVal(VectorRight, 0, 0);
        SetMatVal(VectorRes, 1, 0, tmp);

        tmp = GetMatVal(VectorLeft, 0, 0) * GetMatVal(VectorRight, 1, 0)
            - GetMatVal(VectorLeft, 1, 0) * GetMatVal(VectorRight, 0, 0);
        SetMatVal(VectorRes, 2, 0, tmp);
    }
}

void LinealgInv2(Matrix* A, Matrix* res)
{
    float det = LinealgDet(A);

    SetMatVal(res, 0, 0, GetMatVal(A, 1, 1) / det);
    SetMatVal(res, 1, 1, GetMatVal(A, 0, 0) / det);
    SetMatVal(res, 0, 1, GetMatVal(A, 1, 0) / det);
    SetMatVal(res, 1, 0, GetMatVal(A, 0, 1) / det);
}


// TODO::没完成
/*
void LinealgInv3(Matrix *A, Matrix *res)
{
    float det = LinealgDet(A);
    float A11 = 0.f;
    float A11 = 0.f;
    float A11 = 0.f;
    float A11 = 0.f;
    float A11 = 0.f;
    float A11 = 0.f;
    float A11 = 0.f;
    float A11 = 0.f;
    float A11 = 0.f;

    SetMatVal(res, 0, 0, GetMatVal(A, 1, 1) / det);
    SetMatVal(res, 1, 1, GetMatVal(A, 0, 0) / det);
    SetMatVal(res, 0, 1, GetMatVal(A, 1, 0) / det);
    SetMatVal(res, 1, 0, GetMatVal(A, 0, 1) / det);
}*/

float LinealgDet(Matrix* mat)
{
    if (mat->row == mat->col)
    {
        if (mat->row == 2)
            return GetMatVal(mat, 0, 0) * GetMatVal(mat, 1, 1) -
            GetMatVal(mat, 0, 1) * GetMatVal(mat, 1, 0);

        if (mat->row == 3)
            return GetMatVal(mat, 0, 0) * GetMatVal(mat, 1, 1) * GetMatVal(mat, 2, 2)
            + GetMatVal(mat, 0, 1) * GetMatVal(mat, 1, 2) * GetMatVal(mat, 2, 0)
            + GetMatVal(mat, 0, 2) * GetMatVal(mat, 1, 0) * GetMatVal(mat, 2, 1)
            - GetMatVal(mat, 0, 2) * GetMatVal(mat, 1, 1) * GetMatVal(mat, 2, 0)
            - GetMatVal(mat, 0, 1) * GetMatVal(mat, 1, 0) * GetMatVal(mat, 2, 2)
            - GetMatVal(mat, 0, 0) * GetMatVal(mat, 1, 2) * GetMatVal(mat, 2, 1);
    }
    return 0.0f;
}


void MatrixCopy(const Matrix* src, Matrix* dst)
{
    for (size_t i = 0; i < src->row; i++)
    {
        for (size_t j = 0; j < src->col; j++)
        {
            SetMatVal(dst, i, j, GetMatVal(src, i, j));
        }
    }
}